//
// Created by liangwenhao on 2024/4/4.
//

#include "pid_calculation.h"
#include "math.h"
/**
    PID函数中使用motor->PID_Speed的数据，不传入motor->Motor_Target的数据
    motor->PID_Speed和motor->PID_Angle都是归一后的数据
 **/
void PIDCalculation_Speed(Motor *motor)//速度环PID函数
{
    motor->PID_Speed.err=motor->PID_Speed.Target-motor->PID_Speed.Real;
    if(motor->PID_Speed.err_sum<2)
    {
        motor->PID_Speed.err_sum = motor->PID_Speed.err_sum + motor->PID_Speed.err;
    }
    motor->PID_Speed.POut=motor->PID_Speed.Kp* motor->PID_Speed.err;
    motor->PID_Speed.IOut=motor->PID_Speed.Ki* motor->PID_Speed.err_sum;
    if(motor->PID_Speed.IOut>1)
    {
        motor->PID_Speed.IOut=1;
    }
    else if(motor->PID_Speed.IOut<-1)
    {
        motor->PID_Speed.IOut=-1;
    }
    motor->PID_Speed.DOut=motor->PID_Speed.Kd*(motor->PID_Speed.err-motor->PID_Speed.err_last);
    motor->PID_Speed.Out=motor->PID_Speed.POut+motor->PID_Speed.IOut+motor->PID_Speed.DOut;
    motor->PID_Speed.err_last=motor->PID_Speed.err;
    if(motor->PID_Speed.Out-0.8>0)
    {
        motor->PID_Speed.Out=0.8f;
    }
    else if(motor->PID_Speed.Out+0.8<0)
    {
        motor->PID_Speed.Out=-0.8f;
    }
}

void PIDCalculation_Angle(Motor *motor)//位置环PID函数
{
    motor->PID_Angle.err=motor->PID_Angle.Target-motor->PID_Angle.Real;

    if(fabsf(motor->PID_Angle.err)<0.004)
    {
        motor->PID_Angle.err_sum=motor->PID_Angle.err_sum+motor->PID_Angle.err;
    }
//    motor->PID_Angle.err_sum=motor->PID_Angle.err_sum+motor->PID_Angle.err;
    motor->PID_Angle.POut=motor->PID_Angle.Kp*motor->PID_Angle.err;
    motor->PID_Angle.DOut=motor->PID_Angle.Kd*(motor->PID_Angle.err-motor->PID_Angle.err_last);
    motor->PID_Angle.IOut=motor->PID_Angle.Ki*motor->PID_Angle.err_sum;
    motor->PID_Angle.err_last=motor->PID_Angle.err;
    motor->PID_Angle.Out=motor->PID_Angle.POut+motor->PID_Angle.DOut+motor->PID_Angle.IOut;
    if(motor->PID_Angle.Out>1)
    {
        motor->PID_Angle.Out=1;
    }
    else if(motor->PID_Angle.Out<-1)
    {
        motor->PID_Angle.Out=-1;
    }
}

void PIDCalculation(PID *pid)
{
    pid->err=pid->Target-pid->Real;
    if(pid->err_sum<2)
    {
        pid->err_sum = pid->err_sum + pid->err;
    }
    pid->POut=pid->Kp* pid->err;
    pid->IOut=pid->Ki* pid->err_sum;
    if(pid->IOut>1)
    {
        pid->IOut=1;
    }
    else if(pid->IOut<-1)
    {
        pid->IOut=-1;
    }
    pid->DOut=pid->Kd*(pid->err-pid->err_last);
    pid->Out=pid->POut+pid->IOut+pid->DOut;
    pid->err_last=pid->err;
    if(pid->Out>1)
    {
        pid->Out=1;
    }
    else if(pid->Out<-1)
    {
        pid->Out=-1;
    }
}

void PID_Init(PID *pid,float p,float i,float d)
{
    pid->Kp=p;
    pid->Ki=i;
    pid->Kd=d;
}